import RPi.GPIO as GPIO
import time
import sys
import os
import_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(import_path)
from intelligentCar.motorContler import motorContler
from steeringGearTest.servo import servoContler
from infraredistance.us100carUltrasound import infrared
from evadeObstacle.evadeObstacle import evadeObstacle

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)



class car(motorContler, servoContler, infrared, evadeObstacle):
    def __init__(self):
        motorContler.__init__(self)
        servoContler.__init__(self)
        infrared.__init__(self)
        evadeObstacle.__init__(self)


    def AllStop(self):
        motorContler.motorStop(self)
        GPIO.cleanup()

if __name__ == '__main__':
    try:
        infrared = infrared()
        start_time = None
        evadeObstacle = evadeObstacle()
        car = motorContler()
        while True:
            dist_mov_ave = infrared.DistMeasure()
            [left_measure, right_measure] = evadeObstacle.infra_detect()
            print('Distance:{},left_measure:{}, right_measure:{}'.format(dist_mov_ave, left_measure, right_measure) )

            if (start_time is None) or (time.time() - start_time > 0.5):
                start_time = None
                if left_measure == 0 and right_measure == 1:
                    car.turnLeft(30)
                elif left_measure == 1 and right_measure == 0:
                    car.turnLeft(-30)
                elif left_measure == 0 and right_measure == 0:
                    car.goForward(20)
                else:
                    if int(dist_mov_ave) < 60:
                        car.turnAround(30)
                        start_time = time.time()
                    elif int(dist_mov_ave) < 200:
                        car.goForward(-(dist_mov_ave/2+10))
                    else:
                        car.goForward(-20)
            else:
                pass




    except KeyboardInterrupt:
        print("Measurement stopped by User")
        car.AllStop()
